﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;

namespace CPF_experement
{
    class Silver
    {
        HashSet<Point> RT;
        bool diagonal;
        AgentState[] allAgentsState;
        Hashtable parked;
        int[] allPathCost;
        int maxPathCost;
        public int expanded;
        public int generated;
        public int totalTime;

        public Silver(AgentState[] allAgentsState, bool diagonal)
        {
            RT = new HashSet<Point>();
            parked = new Hashtable();
            this.diagonal = diagonal;
            this.allAgentsState = allAgentsState;
            allPathCost = new int[allAgentsState.Length];
            maxPathCost = 0;
            expanded = 0;
            generated = 0;
            totalTime = 0;
        }

        public bool solve()
        {
            foreach (AgentState agent in allAgentsState)
            {
                agent.currentStep = 0;
                if (!singleAgentA_Star(agent))
                {
                    totalTime = -1;
                    Console.WriteLine("Expanded - " + expanded);
                    Console.WriteLine("Generated - " + generated);
                    Console.WriteLine("No solution found");
                    return false;
                }
            }
            printSum();
            return true;
        }

        private bool singleAgentA_Star(AgentState agent)
        {
            BinaryHeap<AgentState> openList = new BinaryHeap<AgentState>();
            HashSet<AgentState> closedList = new HashSet<AgentState>();
            openList.Add(agent);
            AgentState temp = agent;
            bool valid;
            while (openList.Count > 0)
            {
                if (Run.watch.ElapsedMilliseconds>120000)
                {
                    return false;
                }
                temp = openList.Remove();
                if (temp.huristic == 0)
                {
                    valid = true;
                    for (int i = temp.currentStep; i <= maxPathCost; i++)
                    {
                        if (RT.Contains(new Point(temp.pos_X, temp.pos_Y, -1, i)))
                            valid = false;
                    }
                    if (valid)
                    {
                        reservePath(temp);
                        totalTime += temp.currentStep;
                        //printPath(temp);
                        parked.Add(new Point(temp.pos_X, temp.pos_Y, -1), temp.currentStep);
                        return true;
                    }
                }
                expanded++;
                expendNode(temp, openList, closedList);
            }
            return false;
        }
        private void reservePath(AgentState end)
        {
            AgentState temp = end;
            while (temp != null)
            {
                RT.Add(new Point(temp.pos_X, temp.pos_Y, temp.direction, temp.currentStep));
                allPathCost[temp.agent.agentNum]++;
                temp = temp.prev;
            }
            if (allPathCost[end.agent.agentNum] > maxPathCost)
                maxPathCost = allPathCost[end.agent.agentNum];
        }
        private void expendNode(AgentState node, BinaryHeap<AgentState> openList, HashSet<AgentState> closedList)
        {
            int before_X = node.pos_X;
            int before_Y = node.pos_Y;
            int time = node.currentStep + 1;

            if (isValidMove(before_X, before_Y, time, 0))
            {
                AgentState temp = new AgentState(node);
                AgentState tempCL = temp;
                temp.currentStep = time;
                temp.prev = node;
                if (time > maxPathCost)
                {
                    tempCL = new AgentState(temp);
                    tempCL.currentStep = maxPathCost;
                }
                if (!closedList.Contains(tempCL))
                {
                    closedList.Add(tempCL);
                    temp.currentStep = time;
                    openList.Add(temp);
                    generated++;
                }
            }
            if (isValidMove(before_X - 1, before_Y, time, 1))
            {
                AgentState temp = new AgentState(node);
                AgentState tempCL = temp;
                temp.currentStep = time;
                temp.prev = node;
                temp.move(0, 0);
                temp.direction = 1;
                if (time > maxPathCost)
                {
                    tempCL = new AgentState(temp);
                    tempCL.currentStep = maxPathCost;
                }
                if (!closedList.Contains(tempCL))
                {
                    closedList.Add(tempCL);
                    temp.currentStep = time;
                    openList.Add(temp);
                    generated++;
                }
            }
            if (isValidMove(before_X, before_Y + 1, time, 2))
            {
                AgentState temp = new AgentState(node);
                AgentState tempCL = temp;
                temp.currentStep = time;
                temp.prev = node;
                temp.move(1, 0);
                temp.direction = 2;
                if (time > maxPathCost)
                {
                    tempCL = new AgentState(temp);
                    tempCL.currentStep = maxPathCost;
                }
                if (!closedList.Contains(tempCL))
                {
                    closedList.Add(tempCL);
                    temp.currentStep = time;
                    openList.Add(temp);
                    generated++;
                }
            }
            if (isValidMove(before_X + 1, before_Y, time, 3))
            {
                AgentState temp = new AgentState(node);
                AgentState tempCL = temp;
                temp.currentStep = time;
                temp.prev = node;
                temp.move(2, 0);
                temp.direction = 3;
                if (time > maxPathCost)
                {
                    tempCL = new AgentState(temp);
                    tempCL.currentStep = maxPathCost;
                }
                if (!closedList.Contains(tempCL))
                {
                    closedList.Add(tempCL);
                    temp.currentStep = time;
                    openList.Add(temp);
                    generated++;
                }
            }
            if (isValidMove(before_X, before_Y - 1, time, 4))
            {
                AgentState temp = new AgentState(node);
                AgentState tempCL = temp;
                temp.currentStep = time;
                temp.prev = node;
                temp.move(3, 0);
                temp.direction = 4;
                if (time > maxPathCost)
                {
                    tempCL = new AgentState(temp);
                    tempCL.currentStep = maxPathCost;
                }
                if (!closedList.Contains(tempCL))
                {
                    closedList.Add(tempCL);
                    temp.currentStep = time;
                    openList.Add(temp);
                    generated++;
                }
            }
            if (diagonal)
            {
                if (isValidMove(before_X - 1, before_Y + 1, time, 5))
                {
                    AgentState temp = new AgentState(node);
                    AgentState tempCL = temp;
                    temp.currentStep = time;
                    temp.prev = node;
                    temp.move(4, 0);
                    temp.direction = 5;
                    if (time > maxPathCost)
                    {
                        tempCL = new AgentState(temp);
                        tempCL.currentStep = maxPathCost;
                    }
                    if (!closedList.Contains(tempCL))
                    {
                        closedList.Add(tempCL);
                        temp.currentStep = time;
                        openList.Add(temp);
                        generated++;
                    }
                }
                if (isValidMove(before_X + 1, before_Y + 1, time, 6))
                {
                    AgentState temp = new AgentState(node);
                    AgentState tempCL = temp;
                    temp.currentStep = time;
                    temp.prev = node;
                    temp.move(5, 0);
                    temp.direction = 6;
                    if (time > maxPathCost)
                    {
                        tempCL = new AgentState(temp);
                        tempCL.currentStep = maxPathCost;
                    }
                    if (!closedList.Contains(tempCL))
                    {
                        closedList.Add(tempCL);
                        temp.currentStep = time;
                        openList.Add(temp);
                        generated++;
                    }
                }
                if (isValidMove(before_X + 1, before_Y - 1, time, 7))
                {
                    AgentState temp = new AgentState(node);
                    AgentState tempCL = temp;
                    temp.currentStep = time;
                    temp.prev = node;
                    temp.move(6, 0);
                    temp.direction = 7;
                    if (time > maxPathCost)
                    {
                        tempCL = new AgentState(temp);
                        tempCL.currentStep = maxPathCost;
                    }
                    if (!closedList.Contains(tempCL))
                    {
                        closedList.Add(tempCL);
                        temp.currentStep = time;
                        openList.Add(temp);
                        generated++;
                    }
                }
                if (isValidMove(before_X - 1, before_Y - 1, time, 8))
                {
                    AgentState temp = new AgentState(node);
                    AgentState tempCL = temp;
                    temp.currentStep = time;
                    temp.prev = node;
                    temp.move(7, 0);
                    temp.direction = 8;
                    if (time > maxPathCost)
                    {
                        tempCL = new AgentState(temp);
                        tempCL.currentStep = maxPathCost;
                    }
                    if (!closedList.Contains(tempCL))
                    {
                        closedList.Add(tempCL);
                        temp.currentStep = time;
                        openList.Add(temp);
                        generated++;
                    }
                }
            }
        }
        private bool isValidMove(int pos_X, int pos_Y, int time, int direction)
        {
            if (True_Path_Huristic.isValidTile(pos_X, pos_Y))
            {
                if (!RT.Contains(new Point(pos_X, pos_Y, -1, time)) && !RT.Contains(new Point(pos_X, pos_Y, direction, time).getColaidingPoint()))
                {
                    if (!parked.Contains(new Point(pos_X, pos_Y, -1)) || (int)(parked[new Point(pos_X, pos_Y, 0)]) > time)
                        return true;
                }
            }
            //switch (step.direction)
            //{
            //    case 0:
            //        return true;
            //    case 1:
            //        return safeFromCollision(step.pos_X + 1, step.pos_Y, step.step, 3);
            //    case 2:
            //        return safeFromCollision(step.pos_X, step.pos_Y-1, step.step, 4);
            //    case 3:
            //        return safeFromCollision(step.pos_X - 1, step.pos_Y, step.step, 1);
            //    case 4:
            //        return safeFromCollision(step.pos_X , step.pos_Y+1, step.step, 2);
            //    case 5:
            //        return safeFromCollision(step.pos_X + 1, step.pos_Y-1, step.step, 7);
            //    case 6:
            //        return safeFromCollision(step.pos_X - 1, step.pos_Y-1, step.step, 8);
            //    case 7:
            //        return safeFromCollision(step.pos_X - 1, step.pos_Y+1, step.step, 5);
            //    case 8:
            //        return safeFromCollision(step.pos_X + 1, step.pos_Y+1, step.step, 6);
            //}
            return false;
        }
        //private bool safeFromCollision(int pos_x, int pos_Y, int step, short direction)
        //{
        //    Reservation res = (Reservation)RT[new Reservation(pos_x, pos_Y, step, direction)];
        //    if (res != null && res.direction == direction)
        //        return false;
        //    return true;
        //}
        private void printPath(AgentState final)
        {
            Console.WriteLine(final.agent.ToString());
            Console.WriteLine("-----------------------");
            while (final != null)
            {
                Console.WriteLine(final.ToString());
                final = final.prev;
            }
            Console.WriteLine();
        }
        private void printSum()
        {
            Console.WriteLine("Expanded - " + expanded);
            Console.WriteLine("Generated - " + generated);
            Console.WriteLine("Total cost - " + totalTime);
        }
    }
}